An O(m+n) measure of penetration depth between convex polyhedral bodies for rigid multibody dynamics

In this work, we define a new metric of the distance and depth of penetration between two convex polyhedral bodies. The metric is computed by means of a linear program with three variables and m+n constraints, where m and n are the number of facets of the two polyhedral bodies. As a result, this metric can be computed with O(n+m) algorithmic complexity, superior to the best algorithms known for calculating Euclidean penetration depth. Moreover, our metric is equivalent to the signed Euclidean distance and thus results in the same dynamics when used in the simulation of rigid-body dynamics in the limit of the time step going to 0. We demonstrate the use of this new metric in time-stepping methods for rigid body dynamics with contact and friction.